From 71270226b14733a4b1f2cde58ea9265caa50b38d Mon Sep 17 00:00:00 2001
From: Adrian Panella <ianchi74@outlook.com>
Date: Thu, 9 Mar 2017 09:37:17 +0100
Subject: [PATCH 67/69] generic: Mangle bootloader's kernel arguments

The command-line arguments provided by the boot loader will be
appended to a new device tree property: bootloader-args.
If there is a property "append-rootblock" in DT under /chosen
and a root= option in bootloaders command line it will be parsed
and added to DT bootargs with the form: <append-rootblock>XX.
Only command line ATAG will be processed, the rest of the ATAGs
sent by bootloader will be ignored.
This is usefull in dual boot systems, to get the current root partition
without afecting the rest of the system.

Signed-off-by: Adrian Panella <ianchi74@outlook.com>
---
 arch/arm/Kconfig                        | 11 +++++
 arch/arm/boot/compressed/atags_to_fdt.c | 72 ++++++++++++++++++++++++++++++++-
 init/main.c                             | 16 ++++++++
 3 files changed, 98 insertions(+), 1 deletion(-)

--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1728,6 +1728,17 @@ config ARM_ATAG_DTB_COMPAT_CMDLINE_EXTEN
 	  The command-line arguments provided by the boot loader will be
 	  appended to the the device tree bootargs property.
 
+config ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
+	bool "Append rootblock parsing bootloader's kernel arguments"
+	help
+	  The command-line arguments provided by the boot loader will be
+	  appended to a new device tree property: bootloader-args.
+	  If there is a property "append-rootblock" in DT under /chosen 
+	  and a root= option in bootloaders command line it will be parsed 
+	  and added to DT bootargs with the form: <append-rootblock>XX.
+	  Only command line ATAG will be processed, the rest of the ATAGs
+	  sent by bootloader will be ignored.
+
 endchoice
 
 config CMDLINE
--- a/arch/arm/boot/compressed/atags_to_fdt.c
+++ b/arch/arm/boot/compressed/atags_to_fdt.c
@@ -5,6 +5,8 @@
 
 #if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_EXTEND)
 #define do_extend_cmdline 1
+#elif defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE)
+#define do_extend_cmdline 1
 #else
 #define do_extend_cmdline 0
 #endif
@@ -20,6 +22,7 @@ static int node_offset(void *fdt, const
 	return offset;
 }
 
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
 static int setprop(void *fdt, const char *node_path, const char *property,
 		   void *val_array, int size)
 {
@@ -28,6 +31,7 @@ static int setprop(void *fdt, const char
 		return offset;
 	return fdt_setprop(fdt, offset, property, val_array, size);
 }
+#endif
 
 static int setprop_string(void *fdt, const char *node_path,
 			  const char *property, const char *string)
@@ -38,6 +42,7 @@ static int setprop_string(void *fdt, con
 	return fdt_setprop_string(fdt, offset, property, string);
 }
 
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
 static int setprop_cell(void *fdt, const char *node_path,
 			const char *property, uint32_t val)
 {
@@ -46,6 +51,7 @@ static int setprop_cell(void *fdt, const
 		return offset;
 	return fdt_setprop_cell(fdt, offset, property, val);
 }
+#endif
 
 static const void *getprop(const void *fdt, const char *node_path,
 			   const char *property, int *len)
@@ -58,6 +64,7 @@ static const void *getprop(const void *f
 	return fdt_getprop(fdt, offset, property, len);
 }
 
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
 static uint32_t get_cell_size(const void *fdt)
 {
 	int len;
@@ -69,6 +76,61 @@ static uint32_t get_cell_size(const void
 	return cell_size;
 }
 
+#endif
+
+#if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE)
+
+static char *append_rootblock(char *dest, const char *str, int len, void *fdt)
+{
+	const char *ptr, *end;
+	const char *root="root=";
+	int i, l;
+	const char *rootblock;
+
+	//ARM doesn't have __HAVE_ARCH_STRSTR, so search manually
+	ptr = str - 1;
+
+	do {
+		//first find an 'r' at the begining or after a space
+		do {
+			ptr++;
+			ptr = strchr(ptr, 'r');
+			if(!ptr) return dest;
+
+		} while (ptr != str && *(ptr-1) != ' ');
+
+		//then check for the rest
+		for(i = 1; i <= 4; i++)
+			if(*(ptr+i) != *(root+i)) break;
+
+	} while (i != 5);
+
+	end = strchr(ptr, ' ');
+	end = end ? (end - 1) : (strchr(ptr, 0) - 1);
+
+	//find partition number (assumes format root=/dev/mtdXX | /dev/mtdblockXX | yy:XX )
+	for( i = 0; end >= ptr && *end >= '0' && *end <= '9'; end--, i++);
+	ptr = end + 1;
+
+	/* if append-rootblock property is set use it to append to command line */
+	rootblock = getprop(fdt, "/chosen", "append-rootblock", &l);
+	if(rootblock != NULL) {
+		if(*dest != ' ') {
+			*dest = ' ';
+			dest++;
+			len++;
+		}
+		if (len + l + i <= COMMAND_LINE_SIZE) {
+			memcpy(dest, rootblock, l);
+			dest += l - 1;
+			memcpy(dest, ptr, i);
+			dest += i;
+		}
+	}
+	return dest;
+}
+#endif
+
 static void merge_fdt_bootargs(void *fdt, const char *fdt_cmdline)
 {
 	char cmdline[COMMAND_LINE_SIZE];
@@ -88,18 +150,28 @@ static void merge_fdt_bootargs(void *fdt
 
 	/* and append the ATAG_CMDLINE */
 	if (fdt_cmdline) {
+
+#if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE)
+		//save original bootloader args
+		//and append ubi.mtd with root partition number to current cmdline
+		setprop_string(fdt, "/chosen", "bootloader-args", fdt_cmdline);
+		ptr = append_rootblock(ptr, fdt_cmdline, len, fdt);
+
+#else
 		len = strlen(fdt_cmdline);
 		if (ptr - cmdline + len + 2 < COMMAND_LINE_SIZE) {
 			*ptr++ = ' ';
 			memcpy(ptr, fdt_cmdline, len);
 			ptr += len;
 		}
+#endif
 	}
 	*ptr = '\0';
 
 	setprop_string(fdt, "/chosen", "bootargs", cmdline);
 }
 
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
 static void hex_str(char *out, uint32_t value)
 {
 	uint32_t digit;
@@ -117,6 +189,7 @@ static void hex_str(char *out, uint32_t
 	}
 	*out = '\0';
 }
+#endif
 
 /*
  * Convert and fold provided ATAGs into the provided FDT.
@@ -131,9 +204,11 @@ int atags_to_fdt(void *atag_list, void *
 	struct tag *atag = atag_list;
 	/* In the case of 64 bits memory size, need to reserve 2 cells for
 	 * address and size for each bank */
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
 	__be32 mem_reg_property[2 * 2 * NR_BANKS];
-	int memcount = 0;
-	int ret, memsize;
+	int memsize, memcount = 0;
+#endif
+	int ret;
 
 	/* make sure we've got an aligned pointer */
 	if ((u32)atag_list & 0x3)
@@ -168,7 +243,9 @@ int atags_to_fdt(void *atag_list, void *
 			else
 				setprop_string(fdt, "/chosen", "bootargs",
 					       atag->u.cmdline.cmdline);
-		} else if (atag->hdr.tag == ATAG_MEM) {
+		}
+#ifndef CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE
+		else if (atag->hdr.tag == ATAG_MEM) {
 			if (memcount >= sizeof(mem_reg_property)/4)
 				continue;
 			if (!atag->u.mem.size)
@@ -212,6 +289,10 @@ int atags_to_fdt(void *atag_list, void *
 		setprop(fdt, "/memory", "reg", mem_reg_property,
 			4 * memcount * memsize);
 	}
+#else
+
+	}
+#endif
 
 	return fdt_pack(fdt);
 }
--- a/init/main.c
+++ b/init/main.c
@@ -112,6 +112,10 @@
 
 #include <kunit/test.h>
 
+#if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE)
+#include <linux/of.h>
+#endif
+
 static int kernel_init(void *);
 
 extern void init_IRQ(void);
@@ -992,6 +996,18 @@ asmlinkage __visible void __init __no_sa
 	page_alloc_init();
 
 	pr_notice("Kernel command line: %s\n", saved_command_line);
+
+#if defined(CONFIG_ARM_ATAG_DTB_COMPAT_CMDLINE_MANGLE)
+	//Show bootloader's original command line for reference
+	if(of_chosen) {
+		const char *prop = of_get_property(of_chosen, "bootloader-args", NULL);
+		if(prop)
+			pr_notice("Bootloader command line (ignored): %s\n", prop);
+		else
+			pr_notice("Bootloader command line not present\n");
+	}
+#endif
+
 	/* parameters may set static keys */
 	jump_label_init();
 	parse_early_param();
